#ifndef COMPENSATOR_H
#define COMPENSATOR_H

#include <algorithm>
#include "win_exception.h"
#include <process.h>
#include <iostream>

template<class T_sensor, class T_actuator>
struct compensator
{
  typedef T_sensor sensor_t;
  typedef T_actuator actuator_t;

  compensator(double set_point=0, size_t period=100, sensor_t s=sensor_t(), actuator_t a=actuator_t())
    : t(period), sp(set_point), sig(0), sensor(s), actuator(a), started(false)
  {}
  ~compensator()
  {
    if(started)
      stop();
  }

  void start();
  void stop();

  virtual void setpoint(double sepo)
  {
    sp = sepo;
  }
  virtual double setpoint() const
  {
    return sp;
  }

  virtual void period(size_t p)
  {
    t = p;
  }
  virtual size_t period() const
  {
    return t;
  }

  virtual double signal() const
  {
    return sig;
  }

protected:

  virtual double update(double feedback) = 0;

  double sp;
  size_t t;
  double sig;

  sensor_t sensor;
  actuator_t actuator;

  HANDLE thread;
  unsigned int threadId;
  HANDLE timer;

  bool started;
  
  template<class T_compensator>
  friend unsigned int WINAPI compensator_thread(void* arg);
};


template<class T_compensator>
unsigned int WINAPI compensator_thread(void* arg)
{
  try {
    T_compensator* c = (T_compensator*) arg;

    while(true)
    {
      X_WIN_THROW_IF(WaitForSingleObject(c->timer, INFINITE)!=WAIT_OBJECT_0, "compensator_thread: WaitForSingleObject");
      c->sig = c->update(c->sensor());
      c->actuator(c->sig);
    }
    return 0;
  }
  catch(win_exception& e)
  {
    std::cout<<e.what()<<"\nerror number: "<<e.error_number()<<std::endl;
    return e.error_number();
  }
  catch(std::exception& e)
  {
    std::cout<<e.what()<<std::endl;
    return -1;
  }
}


template <class Ts,class Ta>
void compensator<Ts,Ta>::start()
{
  if(started)
    return;

  started=true;

  timer = CreateWaitableTimer(NULL, FALSE, NULL);
  X_WIN_THROW_IF_NULL(timer, "compensator::compensator: CreateWaitableTimer");
  LARGE_INTEGER Preset;
  Preset.QuadPart = -1;
  X_WIN_THROW_IF_NULL(SetWaitableTimer(timer, &Preset, period(), NULL, NULL, FALSE), "compensator::compensator: SetWaitableTimer");

  thread = (HANDLE) _beginthreadex(NULL, 0, compensator_thread<compensator>, this, 0, &threadId);
  X_WIN_THROW_IF_NULL(thread, "compensator: CreateThread");
  X_WIN_THROW_IF_NULL(SetThreadPriority(thread,THREAD_PRIORITY_TIME_CRITICAL), "compensator: SetThreadPriority");
}


template <class Ts,class Ta>
void compensator<Ts,Ta>::stop()
{
  if(!started)
    return;

  started=false;

  X_WIN_THROW_IF_NULL(TerminateThread(thread,0), "~tracker: TerminateThread(thread)");
  
  DWORD ret=STILL_ACTIVE;
  while(true)
  {
    X_WIN_THROW_IF_NULL(GetExitCodeThread(thread,&ret), "~tracker: WaitForSingleObject");
    if(ret!=STILL_ACTIVE)
      break;
    Sleep(period());
  }
  X_WIN_THROW_IF_NULL(CloseHandle(thread), "~tracker: CloseHandle(thread)");
  X_WIN_THROW_IF_NULL(CloseHandle(timer), "tracker::~tracker: CloseHandle(timer)");
}



template<class T_sensor, class T_actuator>
struct proportional: virtual public compensator<T_sensor, T_actuator>
{
  proportional(double set_point=0, size_t period=100, double kp = 1., sensor_t s=sensor_t(), actuator_t a=actuator_t())
    : compensator(set_point, period, s, a), Kp_(kp)
  {}

  virtual void Kp(double kp)
  {
    Kp_ = kp;
  }
  virtual double Kp() const
  {
    return Kp_;
  }

protected:
  virtual double update(double feed_back)
  {
    return Kp_ * (sp-feed_back);
  }

  double Kp_;
};


template<class T_sensor, class T_actuator>
struct derivative: virtual public compensator<T_sensor, T_actuator>
{
  derivative(double kd = 1., size_t period=100, sensor_t s=sensor_t(), actuator_t a=actuator_t())
    : compensator(0,period,s,a), Kd_(kd), fb_prev(0)
  {}

  virtual void Kd(double kd)
  {
    Kd_ = kd;
  }
  virtual double Kd() const
  {
    return Kd_;
  }

protected:
  virtual double update(double fb)
  {
    double signal = Kd_*(fb-fb_prev)/t;
    fb_prev = fb;
    return signal;
  }

  double Kd_;
  double fb_prev;
};


template<class T_sensor, class T_actuator>
struct integrative: virtual public compensator<T_sensor, T_actuator>
{
  integrative(double set_point=0, size_t period=100, double ki = 1., sensor_t s=sensor_t(), actuator_t a=actuator_t())
    : compensator(set_point,period,s,a), Ki_(ki), int_prev(0), error_prev(0)
  {}

  virtual void Ki(double ki)
  {
    Ki_ = ki;
  }
  virtual double Ki() const
  {
    return Ki_;
  }

  virtual void zero_int()
  {
    int_prev=0;
  }

protected:
  virtual double update(double fb)
  {
    double signal = ( int_prev + Ki_ * (t/2)*(error_prev + (sp-fb)) );
    error_prev = sp-fb;
    int_prev = signal;
    return signal;
  }
  double Ki_;
  double int_prev;
  double error_prev;
};


template<class T_sensor, class T_actuator>
struct PID: 
  public proportional<T_sensor, T_actuator>, 
  public integrative<T_sensor, T_actuator>, 
  public derivative<T_sensor, T_actuator>
{
  PID(const proportional& p=proportional(), const integrative& i=integrative(), const derivative& d=derivative())
    : proportional(p), integrative(i), derivative(d)
  {}

protected:
  virtual double update(double fb)
  {
    return proportional::update(fb) - derivative::update(fb) + integrative::update(fb);
  }
};


template<class T_compensator>
struct saturator: public T_compensator
{
  typedef T_compensator compensator_t;

  saturator(const compensator_t& comp = compensator_t(), double min = -1., double max = 1.)
    : compensator(comp), min_(min), max_(max)
  {
    X_THROW_INV_ARG_IF(min>=max, "saturater: min>=max");
  }

  virtual void set_min(double d)
  {
    min_ = d;
  }
  virtual double get_min() const
  {
    return min_;
  }

  virtual void set_max(double d)
  {
    max_ = d;
  }
  virtual double get_max() const
  {
    return max_;
  }

protected:
  virtual double update(double feedback)
  {
    return std::min(std::max(compensator_t::update(feedback),min_), max_);
  }
  double min_;
  double max_;
};

#endif